
#ifndef NAV_DWB_CONTROLLER_PKG__TRAJECTORY_UTILS_HPP_
#define NAV_DWB_CONTROLLER_PKG__TRAJECTORY_UTILS_HPP_

#include "rclcpp/rclcpp.hpp"
#include "dwb_msgs/msg/trajectory2_d.hpp"

namespace nav_dwb_controller_pkg
{

  /**
   * @brief Helper function to find a pose in the trajectory with a particular time time_offset
   * @param trajectory The trajectory to search
   * @param time_offset The desired time_offset
   * @return reference to the pose that is closest to the particular time offset
   *
   * Linearly searches through the poses. Once the poses time_offset is greater than the desired time_offset,
   * the search ends, since the poses have increasing time_offsets.
   */
  const geometry_msgs::msg::Pose2D &getClosestPose(
      const dwb_msgs::msg::Trajectory2D &trajectory,
      const double time_offset);

  /**
   * @brief Helper function to create a pose with an exact time_offset by linearly interpolating between existing poses
   * @param trajectory The trajectory with pose and time offset information
   * @param time_offset The desired time_offset
   * @return New Pose2D with interpolated values
   * @note If the given time offset is outside the bounds of the trajectory, the return pose will be either the first or last pose.
   */
  geometry_msgs::msg::Pose2D projectPose(
      const dwb_msgs::msg::Trajectory2D &trajectory,
      const double time_offset);

} // namespace nav_dwb_controller_pkg

#endif // NAV_DWB_CONTROLLER_PKG__TRAJECTORY_UTILS_HPP_
